#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Hubs,  S4, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     SMUX,           sensorI2CCustom9V)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     mtrFL,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     mtrBL,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     collector,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     mtrBR,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S4_C1_2,     mtrFR,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S4_C2_1,     mtrLifterL,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S4_C2_2,     mtrLifterR,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    goalGrabber,          tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    container,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Doesn't compile, because you have to change other code for that.
//If this is needed, change includes, then change back

#define PARALLEL_PAIRS

#include "../include_files/auto_includes.c"

task main()
{
	InitializeAuto();
	waitForStart();

	//Turn test

	//TurnLeftDegrees(90, 50);
	//wait1Msec(1000);
	//TurnRightDegrees(180, 50);
	//wait1Msec(1000);
	//TurnLeftDegrees(180, 50);
	//wait1Msec(1000);
	//TurnRightDegrees(90, 50);


	//Gyro drive test
	//wait1Msec(5000);
	DriveForwardDistanceGyro(1100,50);

	/*
	//Gyro rotation display
	StopAllDriveMotors();
	wait1Msec(250); //The wait is here to ensure the robot comes to a stop before calibrating the gyro
	ResetEncoderValue();
	startGyro();
	while(!gyroValid()) {} // @todo should have a timeout here
	resetGyro();
	ClearTimer(T4);
	bool failed = false;
	while(true)
	{
		nxtDisplayTextLine(1, "%d", HTIRS2readDCDir(irSensor));
	}*/
}
